﻿//MIT License
//Copyright (c) 2024 nuaazgg, Nanjing University of Aeronautics and Astronautics(NUAA)

using System;
using System.IO.Ports;
using System.Linq;
using UnityEngine;

public class RobotHand
{
    private SerialPort serial;
    private byte[] sendData;
    private byte[] getData;

    public RobotHand(string com, int baudRate)
    {
        serial = new SerialPort(com, baudRate, Parity.None, 8, StopBits.One);
        serial.Open();
        Debug.Log(serial.IsOpen);
        serial.ReadTimeout = 500;
        serial.WriteTimeout = 500;
        sendData = new byte[20];
        //包头
        sendData[0] = 0xEB;
        sendData[1] = 0x90;
        //handID
        sendData[2] = 0x01;
        //数据个数
        sendData[3] = 0x0F;
        //写操作
        sendData[4] = 0x12;
        //地址
        sendData[5] = 0xCE;
        sendData[6] = 0x05;

        getData = new byte[9];
        getData[0] = 0xEB;
        getData[1] = 0x90;
        getData[2] = 0x01;
        getData[3] = 0x04;
        getData[4] = 0x11;
        getData[5] = 0x0A;
        getData[6] = 0x06;
        getData[7] = 0x0C;
        getData[8] = Checknum(getData);
    }

    ~RobotHand() //析构函数
    {
        if(serial.IsOpen)
            serial.Close();
    }

    byte Checknum(byte[] data)
    {
        byte result = 0;
        int length = data.Length - 1;//校验和，是除应答帧头外其余数据的累加和的低字节,最后以为是填校验和的
        for (int i = 2; i < length; ++i)
        {
            result += data[i];
        }
        return result;
    }

    public void SetAngle(short[] angle)//分别是小拇指 无名指 中指 食指 大拇指弯曲  大拇指旋转
    {
/*        if (angle[0] < -1 || angle[0] > 1000)
            return;
        if (angle[1] < -1 || angle[1] > 1000)
            return;
        if (angle[2] < -1 || angle[2] > 1000)
            return;
        if (angle[3] < -1 || angle[3] > 1000)
            return;
        if (angle[4] < -1 || angle[4] > 1000)
            return;
        if (angle[5] < -1 || angle[5] > 1000)
            return;*/
        //已进行缩放，保证再0-1000
        angle[5] = 1000;
        if (serial.BytesToRead != 0)
            serial.ReadExisting();
        for (int i = 0; i < 6; ++i)
        {
            var HexBytes = BitConverter.GetBytes(angle[i]);
            sendData[7+i*2] = HexBytes[0];
            sendData[8+i*2] = HexBytes[1];
        }
        sendData[19] = Checknum(sendData);

        serial.Write(sendData, 0, 20);
        //这里不浪费时间去读取返回数据了
    }

    public int[] GetAngle()
    {
        byte[] readData = new byte[20];
        int[] actangle = new int[6] { -1,-1,-1,-1,-1,-1 };
        int a = 5;
        Debug.Log(serial.BytesToRead);
        if (serial.BytesToRead != 0)
            serial.ReadExisting();
        serial.Write(getData, 0, 9);
        System.Threading.Thread.Sleep(10);//保证机械手串口数据返回时间，5次读不到就算了
        while (a > 0)
        {
            if (serial.BytesToRead == 20)
                break;
            System.Threading.Thread.Sleep(5);
            --a;
        }
        if(a == 0)
        {
            return actangle;
        }
        serial.Read(readData, 0, 20);
        
        for (int i = 1; i < 7; i++)
        {
            if (readData[i * 2 + 5] == 0xff && readData[i * 2 + 6] == 0xff)
                actangle[i - 1] = -1;
            else
                actangle[i - 1] = readData[i * 2 + 5] + (readData[i * 2 + 6] << 8);
        }
        return actangle;

    }

    public void Stop()
    {
        serial.Close();
    }

}
